#include <boost/foreach.hpp>
#include "PatrolPlanner.h"

#define forEach BOOST_FOREACH

/**
 * Constructor- initializes object with given values
 * and initializes make-plan service
 */
PatrolPlanner::PatrolPlanner(ros::NodeHandle& node, std::vector<float> xLocs,
		std::vector<float> yLocs, const float& goalTolerance) throw (int) : m_node(node) {

	// set members
    m_serviceName = "move_base_node/make_plan";
	m_xLocs = xLocs;
	m_yLocs = yLocs;
	m_goalTolerance = goalTolerance;
	m_worldFrame = "map";

    // initialize service for make plan
	while (!ros::service::waitForService(m_serviceName, ros::Duration(3.0))) {
		ROS_INFO("Waiting for service move_base/make_plan to become available");
	}

	// initialize client
	m_client = m_node.serviceClient<nav_msgs::GetPlan>(m_serviceName, true);
	if (!m_client) {
		ROS_FATAL("Could not initialize get plan service from %s",
				m_client.getService().c_str());
		throw -1;
	}

	// create positions out of given coordinates
	cordsToPos();
}

/**
 * Destructor
 */
PatrolPlanner::~PatrolPlanner() { }

/**
 * Plans patrol path using Nearest Neighbor approximation:
 * 1. sets the closest patrol location to robot's initial position as start
 * 2. finds closest location to start
 * 3. finds closest location to recently added location, until path is done
 * 4. sets in output parameters the coordinates of the path locations and cost of each segment
 */
void PatrolPlanner::planPatrolPath(const geometry_msgs::Point& initPos,
		std::vector<geometry_msgs::Point>& patrolPath,
		std::vector<float>& segmentsCost, float& initCost) {

	// initialize list of indices of patrol path locations
	std::vector<int> patrolIndices;

	// calculate cost matrix
	float** costMatrix = calcCostMatrix();

	// initialize list of indices of patrol locations not assigned to patrol path
	std::vector<int> remainingLocs;
	for (int i = 0 ; i < m_xLocs.size() ; ++i) {
		remainingLocs.push_back(i);
	}

	// find index of staring patrol location
	int remLocIndex;
	int patrolStart = closestPatrolLoc(initPos, remainingLocs, remLocIndex);
	remainingLocs.erase(remainingLocs.begin() + remLocIndex);

	// add index to patrol path
	patrolIndices.push_back(patrolStart);

	// build patrol path according to nearest neighbor approximation
	int currLocIndex = patrolStart;
	while (remainingLocs.size() > 0) {

		// get index of closest patrol location to current one
		int nextLocIndex = closestPatrolLoc(costMatrix, currLocIndex, remainingLocs, remLocIndex);
		remainingLocs.erase(remainingLocs.begin() + remLocIndex);

		// add index to patrol path
		patrolIndices.push_back(nextLocIndex);

		// move to next location
		currLocIndex = nextLocIndex;
	}

	// generate path out of indices
	genPath(patrolIndices, patrolPath);

	// calculate cost of each path segment
	calcSegmentsCost(costMatrix, patrolIndices, segmentsCost);

	// calculate cost from initial position to patrol's starting point
	initCost = getPathCost(initPos, patrolPath[0]);

	// delete cost matrix
	for (int i = 0 ; i < m_xLocs.size() ; ++i) {
		delete[] costMatrix[i];
	}
	delete[] costMatrix;
}

/**
 * Converts coordinates of patrol locations to positions
 */
void PatrolPlanner::cordsToPos() {

	// convert each coordinate to a position
	for (int i = 0 ; i < m_xLocs.size() ; ++i) {

		// create a new position with current coordinates
		geometry_msgs::Point currPos;
		currPos.x = m_xLocs[i];
		currPos.y = m_yLocs[i];
		currPos.z = GOAL_ROT;

		// add new position
		m_patrolLocs.push_back(currPos);
	}
}

/**
 * For each patrol location, calculates cost of path between
 * the location and all other locations
 */
float** PatrolPlanner::calcCostMatrix() {

	// initialize cost matrix
	float** costMatrix = new float*[m_patrolLocs.size()];
	for (int i = 0 ; i < m_patrolLocs.size() ; ++i) {
		costMatrix[i] = new float[m_patrolLocs.size()];
	}

	// iterate over all patrol locations
	for (int src = 0 ; src < m_patrolLocs.size() ; ++src) {
		for (int goal = 0 ; goal < m_patrolLocs.size() ; ++goal) {

			// skip trivial path
			if (src == goal) {
				costMatrix[src][goal] = 0.0;
				continue;
			}

			// set path cost between current locations
			costMatrix[src][goal] = getPathCost(m_patrolLocs[src], m_patrolLocs[goal]);
		}
	}

	// return result
	return costMatrix;
}

/**
 * Returns index of closest patrol location to given map position
 */
int PatrolPlanner::closestPatrolLoc(const geometry_msgs::Point& initPos,
		std::vector<int>& remainingLocs, int& index) {

	// initialize closest patrol location
	int closestLoc = remainingLocs[0];
	index = 0;
	float minCost = getPathCost(initPos, m_patrolLocs[closestLoc]);

	// find closest patrol location
	for (int i = 1 ; i < remainingLocs.size() ; ++i) {

		// calculate cost between given position and next remaining patrol location
		float currCost = getPathCost(initPos, m_patrolLocs[remainingLocs[i]]);

		// update closest patrol location
		if (currCost < minCost) {
			minCost = currCost;
			closestLoc = remainingLocs[i];
			index = i;
		}
	}

	// return index of closest patrol location
	return closestLoc;
}

/**
 * Returns index of closest patrol location to given patrol location
 */
int PatrolPlanner::closestPatrolLoc(float** const costMatrix,
		const int& srcLocIndex, std::vector<int>& remainingLocs, int& index) {

	// initialize closest patrol location
	int closestLoc = remainingLocs[0];
	index = 0;
	float minCost = costMatrix[srcLocIndex][closestLoc];

	// find closest patrol location
	for (int i = 1 ; i < remainingLocs.size() ; ++i) {

		// update closest patrol location
		if (costMatrix[srcLocIndex][remainingLocs[i]] < minCost) {
			minCost = costMatrix[srcLocIndex][remainingLocs[i]];
			closestLoc = remainingLocs[i];
			index = i;
		}
	}

	// return index of closest patrol location
	return closestLoc;
}

/**
 * Returns sum of distances between each adjacent pair of positions
 * along the planned path
 */
float PatrolPlanner::getPathCost(const geometry_msgs::Point& srcPos,
		const geometry_msgs::Point& goalPos) {

	// initialize service query
	nav_msgs::GetPlan planSrv;
	fillPathRequest(srcPos, goalPos, planSrv.request);

	// verify service connection
	if (!m_client) {
		ROS_FATAL("Persistent service connection to %s failed",
				m_client.getService().c_str());
		ROS_ASSERT(0);
	}

	// call service
	bool success = m_client.call(planSrv);

	// check success
	ROS_ASSERT_MSG(success, "Could not plan path");
	ROS_ASSERT_MSG(!planSrv.response.plan.poses.empty(), "Got empty plan, cannot plan patrol");

	// initialize current position with first position along the path
	geometry_msgs::PoseStamped prevPos = planSrv.response.plan.poses.at(0);

	// sum distance between each pair of path positions
	float cost = 0.0;
	forEach(const geometry_msgs::PoseStamped &currPos, planSrv.response.plan.poses) {

		// calculate distance between current positions
		tf::Vector3 prevVec = positionToVec(prevPos.pose.position);
		tf::Vector3 currVec = positionToVec(currPos.pose.position);
		float currDist = currVec.distance(prevVec);

		// sum distance
		cost += currDist;

		// move to next position
		prevPos = currPos;
	}

	// return path cost
	return cost;
}

/**
 * Stores in given output parameters a sorted copy of x and y coordinates
 * of the patrol locations according to given indices list
 */
void PatrolPlanner::genPath(std::vector<int> patrolIndices,
		std::vector<geometry_msgs::Point>& patrolPath) {

	// sort patrol locations as indicated by indices list
	for (int i = 0 ; i < patrolIndices.size() ; ++i) {
		patrolPath.push_back(m_patrolLocs[patrolIndices[i]]);
	}
}

/**
 * Calculates cost of each segment between indicated patrol locations
 */
void PatrolPlanner::calcSegmentsCost(float** const costMatrix,
		std::vector<int> patrolIndices, std::vector<float>& segmentsCost) {

	// set cost of segment from last location to first
	int lastLocIndex = patrolIndices.size() - 1;
	segmentsCost.push_back(costMatrix[patrolIndices[lastLocIndex]][patrolIndices[0]]);

	// iterate over adjacent pairs of indices
	for (int i = 0 ; i < lastLocIndex ; ++i) {

		// set current segment's cost
		segmentsCost.push_back(costMatrix[patrolIndices[i]][patrolIndices[i+1]]);
	}
}

/**
 * Sets values for make-plan service request for the desired path
 */
void PatrolPlanner::fillPathRequest(const geometry_msgs::Point& srcPos,
		const geometry_msgs::Point& goalPos,
		nav_msgs::GetPlan::Request &request) {

	// set source position
    request.start.header.frame_id = m_worldFrame;
    request.start.pose.position.x = srcPos.x;
    request.start.pose.position.y = srcPos.y;
    request.start.pose.orientation = angleToQuat(srcPos.z);

    // set goal position
    request.goal.header.frame_id = m_worldFrame;
    request.goal.pose.position.x = goalPos.x;
    request.goal.pose.position.y = goalPos.y;
    request.goal.pose.orientation = angleToQuat(goalPos.z);

    // set goal tolerance
    request.tolerance = m_goalTolerance;
}

/**
 * Returns a tf::Vector3 with given position's values
 */
tf::Vector3 PatrolPlanner::positionToVec(const geometry_msgs::Point& position) {
	return tf::Vector3(position.x, position.y, position.z);
}

/**
 * Returns quaternion matching given angle
 */
geometry_msgs::Quaternion PatrolPlanner::angleToQuat(const float& angle) {

	// convert angle to radians
	double radians = angle * (M_PI/180);

	// return quaternion
	tf::Quaternion quaternion;
	quaternion = tf::createQuaternionFromYaw(radians);
	geometry_msgs::Quaternion qMsg;
	tf::quaternionTFToMsg(quaternion, qMsg);

	return qMsg;
}
